/*
 * @Description: PGV通信、数据处理线程，PGV传感器数据收发为一发一收
 * @Author: Dryad
 * @Date: 2019-05-27 22:34:34
 */
#include <rthw.h>
#include <rtdevice.h>
#include <arm_math.h>
#include "./PGV.h"

#define _BV(n) (1 << n) /* 移位宏 */

struct PGV_Receive_Data
{
    char PGV_state[2];    /* 读头状态 */
    char x_data[4];       /* x偏差数据 */
    char y_data[2];       /* y偏差数据 */
    char reserve9_10[2];  /* 保留 */
    char angle_data[2];   /* 角度偏差数据 */
    char reserve13_14[2]; /* 保留 */
    char code[4];         /* 控制码和Tag标签号 */
    char warning[2];      /* 报警代码 */
    char xor_code;        /* 校验 */
};

/* PGV测量数据 */
struct PGV_measure_data
{
    float delta_x;
    float delta_y;
    float delta_theta;
};

static struct PGV_measure_data measure_inpgv;        /* 测量数据在PGV坐标系中的值和在AGV坐标系中的值 */
static float pgv_x_coor, pgv_y_coor, pgv_theta_coor; /* 传感器在AGV中的坐标 */

static rt_err_t Analyze_Read_CMD_Data(const char *source_data, int length, struct PGV_measure_data *data_inpgv); /* 解析PGV读取数据指令应答数据 */
static rt_err_t Analyze_Set_Color(const enum PGV_CMD_Mode _cmd, const char *source, const char *target);         /* 解析PGV设置色带颜色应答数据 */
static rt_err_t Analyze_Set_Dir(const enum PGV_CMD_Mode _cmd, const char *source, int length);                   /* 解析PGV设置方向应答数据 */
static void Trans_Coor(const struct PGV_measure_data *const data_inpgv, struct PGV_Response_Data *data_inagv);   /* 将数据从传感器坐标转换到AGV坐标 */

static rt_size_t pgv_rx_length; /* PGV接收到的数据长度计数 */
ALIGN(RT_ALIGN_SIZE)
static char pgv_rx_data[80];                   /* PGV接收数据 */
static enum PGV_CMD_Mode tx_cmd;               /* 需要发送的指令 */
static struct PGV_Response_Data response_data; /*PGV应答数据*/

static struct rt_messagequeue pgv_rx_data_message; /* 保存PGV读头数据消息队列，用于向其他线程发送数据 */
static struct rt_semaphore pgv_rx_sem;             /* 接收到应答数据信号量 */
static struct rt_mailbox pgv_tx_cmd_mailbox;       /* PGV待发送指令邮箱，用于接收其他线程数据 */
ALIGN(RT_ALIGN_SIZE)
static rt_uint32_t pgv_tx_cmd_mailbox_pool[8]; /* PGV待发送指令邮箱内存 */
#define MESSAGE_SIZE_MAX 32                    /* 消息队列最大数目 */
ALIGN(RT_ALIGN_SIZE)
static char pgv_rx_data_message_pool[(sizeof(struct PGV_Response_Data) + 4) * MESSAGE_SIZE_MAX]; /* PGV消息队列内存，数据大小+消息头指针 */

static void PGV_Control_Thread(void *parameter);               /* PGV控制线程 */
static rt_err_t PGV_CallBack(rt_device_t dev, rt_size_t size); /* 接收回调函数 */
static struct rt_thread pgv_control_thread;                    /* PGV线程句柄 */
ALIGN(RT_ALIGN_SIZE)                                           /* 线程栈4字节对齐 */
static char pgv_control_thread_stack[1024];                    /* 线程栈起始地址 */
static rt_device_t pgv_dev = RT_NULL;

/**
 * @description: 解析PGV读取数据指令应答数据
 * @param {type} 
 * @return: 解析错误，返回-RT_ERROR，解析正确，返回RT_EOK
 */
rt_err_t Analyze_Read_CMD_Data(const char *source_data, int length, struct PGV_measure_data *data_inpgv)
{
    const char *data = 0; /* 指向待校验数据头 */

    /* 接收数据不足21个字节 */
    if (length < 21)
    {
        return -RT_ERROR;
    }

    data = (const char *)&source_data[length - 21]; /* 可能会接收到多个数据帧，处理最后一帧数据，后续需要在实验中测试 */
    char xor_temp = 0;                              /* 校验码暂存 */

    for (int j = 0; j < 20; j++)
    {
        xor_temp ^= data[j]; /* 前20个字节的异或校验 */
    }

    /* 校验码错误 */
    if ((char)xor_temp != (char)data[20])
        return -RT_ERROR;

    response_data.cmd = Read_PGV_Data;                                               /* 当前为读取数据指令 */
    const struct PGV_Receive_Data *pgv_data = (const struct PGV_Receive_Data *)data; /* 强制类型转换 */

    char _flag = (pgv_data->PGV_state[0] & _BV(2)); /* 提取警告标志 */
    /* 存在警告代码 */
    if (_flag)
    {
        response_data.warn_code.code = (pgv_data->warning[0] << 7 | pgv_data->warning[1]); /* 错误代码 */
    }
    else
    {
        response_data.warn_code.code = 0;
    }

    if ((pgv_data->PGV_state[1] & _BV(6)) == _BV(6)) /* 检测到tag标签 */
    {
        response_data.target = Data_Matrix_Tag; /* 检测到tag标签 */
    }
    else
    {
        char _target = 0;
        _target = (pgv_data->PGV_state[0] & _BV(1)) | (pgv_data->PGV_state[1] & _BV(2)); /* 提取np位和nl位 */
        switch (_target)
        {
        case 0:        /* 同时识别到位置码带和色带，但以位置码带为准，忽略色带 */
        case (_BV(2)): /* 识别到位置码带 */
            response_data.target = Code_Tape;
            break;
        case (_BV(1)): /* 识别到有效色带 */
            response_data.target = Colored_Tape;
            break;
        case ((_BV(1)) | (_BV(2))): /* 读头没有识别到码带，颜色轨道，tag标签 */
        default:
            response_data.target = NO_Target;
            return RT_EOK;
        }
    }

    rt_int32_t data_temp;
    rt_int32_t x_temp, y_temp, angle_temp;

    /* 提取返回指令中的数据，得到x,y,angle,tag_control_num */

    /* x偏差 */
    data_temp = ((pgv_data->x_data[0] & 0x07) << 21) + (pgv_data->x_data[1] << 14) + (pgv_data->x_data[2] << 7) + pgv_data->x_data[3];
    x_temp = (data_temp & _BV(23)) ? (rt_int32_t)(-(_BV(24) - data_temp)) : (rt_int32_t)data_temp;

    /* y偏差 */
    data_temp = (pgv_data->y_data[0] << 7) + pgv_data->y_data[1];
    y_temp = (data_temp & _BV(13)) ? (rt_int32_t)(-(_BV(14) - data_temp)) : (rt_int32_t)data_temp;

    /* angle偏差 */
    data_temp = (pgv_data->angle_data[0] << 7) + pgv_data->angle_data[1];
    angle_temp = (data_temp & _BV(13)) ? (rt_int32_t)(-(_BV(14) - data_temp)) : (rt_int32_t)data_temp;

    switch (response_data.target)
    {
    case Data_Matrix_Tag: /* 检测到tag数据 */
    {
        /* tag_control_num tag标签号或者控制码 */
        data_temp = ((pgv_data->code[0]) << 21) + (pgv_data->code[1] << 14) + (pgv_data->code[2] << 7) + pgv_data->code[3];
        response_data.data.dm_data.tag_control_num = (pgv_data->PGV_state[0] & _BV(3)) ? ((data_temp >> 14) & 0x3fff) : data_temp;

        data_inpgv->delta_x = x_temp / 10.0f;
        data_inpgv->delta_y = y_temp / 10.0f;
        data_inpgv->delta_theta = angle_temp / 10.0f;
    }
    break;
    case Colored_Tape: /* 检测到色带 */
    {
        if (angle_temp > 1800)
        {
            angle_temp -= 3600;
        }

        data_inpgv->delta_x = 0.0f;
        data_inpgv->delta_y = y_temp / 10.0f;
        data_inpgv->delta_theta = angle_temp / 10.0f;
    }
    break;
    case Code_Tape: /* 检测到位置码带 */
    {
        data_inpgv->delta_x = x_temp / 10.0f;
        data_inpgv->delta_y = y_temp / 10.0f;
        data_inpgv->delta_theta = angle_temp / 10.0f;
    }
    break;
    default: /* 未识别到tag标签和色带 */
    {
        response_data.target = NO_Target;
    }
    break;
    }

    return RT_EOK; /* 解析正确 */
}

/**
 * @description: 解析PGV设置色带颜色应答数据
 * @param {type} 
 * @return: 
 */
rt_err_t Analyze_Set_Color(const enum PGV_CMD_Mode _cmd, const char *source, const char *target)
{
    char *str = rt_strstr(source, target);
    if (str != RT_NULL)
    {
        /* 找到应答 */
        response_data.cmd = _cmd;
        response_data.target = NO_Target;
        response_data.warn_code.code = 0;
        return RT_EOK;
    }
    else
    {
        return -RT_ERROR;
    }
}

/**
 * @description: 解析PGV设置方向应答数据
 * @param {type} 
 * @return: 
 */
rt_err_t Analyze_Set_Dir(const enum PGV_CMD_Mode _cmd, const char *source, int length)
{
    char target = 0;
    switch (_cmd)
    {
    case Set_Dir_R:
    case Set_Dir_L:
    case Set_Dir_Best:
        target = (char)_cmd;
        break;
    default:
        return -RT_ERROR;
        // break;
    }

    char xor_temp;
    for (int i = 1; i < length - 1; i++)
    {
        if (source[i] == target)
        {
            /* 找到了目标字符 */
            xor_temp = source[i - 1] ^ source[i];
            if (xor_temp == source[i + 1])
            {
                /* 校验正确 */
                response_data.cmd = _cmd;
                response_data.target = NO_Target;
                response_data.warn_code.code = 0;
                return RT_EOK;
            }
        }
    }

    return -RT_ERROR;
}

void Trans_Coor(const struct PGV_measure_data *const data_inpgv, struct PGV_Response_Data *data_inagv)
{
    switch (data_inagv->target)
    {
    case Data_Matrix_Tag: /* 检测到tag数据 */
    {
        data_inagv->data.dm_data.x = data_inpgv->delta_x;
        data_inagv->data.dm_data.y = data_inpgv->delta_y;
        data_inagv->data.dm_data.angle = 90.0f - data_inpgv->delta_theta;

        data_inagv->data.dm_data.x -= pgv_x_coor;
        data_inagv->data.dm_data.y -= pgv_y_coor;
        data_inagv->data.dm_data.angle -= pgv_theta_coor;
    }
    break;
    case Colored_Tape: /* 检测到色带 */
    {
        data_inagv->data.colored_tape_data.angle = -data_inpgv->delta_theta;
        float temp = data_inagv->data.dm_data.angle / 180.0f * PI;
        data_inagv->data.colored_tape_data.delta = data_inpgv->delta_y * arm_sin_f32(temp);

        data_inagv->data.colored_tape_data.delta -= pgv_x_coor;
        data_inagv->data.colored_tape_data.angle -= pgv_theta_coor;
    }
    break;
    case Code_Tape: /* 检测到位置码带 */
    {
        data_inagv->data.code_tape_data.angle = -data_inpgv->delta_theta;
        data_inagv->data.code_tape_data.delta_y = data_inpgv->delta_x;
        data_inagv->data.code_tape_data.x = -data_inpgv->delta_y;

        data_inagv->data.code_tape_data.x -= pgv_x_coor;
        data_inagv->data.code_tape_data.delta_y -= pgv_y_coor;
        data_inagv->data.code_tape_data.angle -= pgv_theta_coor;
    }
    break;
    default: /* 未识别到tag标签和色带 */
        break;
    }
}

/**
 * @description: PGV收发线程初始化
 * @param {type} 
 * @return: 
 */
void PGV_Thread_Init(float x, float y, float theta)
{
    rt_err_t rt_result = RT_EOK;

    pgv_x_coor = x;
    pgv_y_coor = y;
    pgv_theta_coor = theta;

    pgv_dev = rt_device_find(PGV_DEVICE_NAME);

    if (pgv_dev == RT_NULL)
    {
        rt_kprintf("PGV Uart Error\n");
        return;
    }
    /* 设置波特率 */
    struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
    config.data_bits = DATA_BITS_9;
    config.stop_bits = STOP_BITS_1;
    config.parity = PARITY_EVEN;
    config.bufsz = 128;

    rt_device_control(pgv_dev, RT_DEVICE_CTRL_CONFIG, &config);

    rt_device_open(pgv_dev, RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_DMA_RX); /*中断发送，DMA接收打开串口*/

    /* 初始化PGV接收数据信号量，初始值为0，接收到数据后释放信号量 */
    rt_sem_init(&pgv_rx_sem, "pgv received sem", 0, RT_IPC_FLAG_FIFO);
    /* 初始化PGV数据发送邮箱，一封邮件占4个字节 */
    rt_mb_init(&pgv_tx_cmd_mailbox, "pgv send mailbox", pgv_tx_cmd_mailbox_pool, sizeof(pgv_tx_cmd_mailbox_pool) / sizeof(rt_uint32_t), RT_IPC_FLAG_FIFO);
    /* 初始化PGV数据读取消息队列 */
    rt_mq_init(&pgv_rx_data_message, "pgv response data",
               pgv_rx_data_message_pool, sizeof(struct PGV_Response_Data), sizeof(pgv_rx_data_message_pool), RT_IPC_FLAG_FIFO);

    rt_device_set_rx_indicate(pgv_dev, PGV_CallBack);

    rt_result = rt_thread_init(&pgv_control_thread, "pgv thread", PGV_Control_Thread, RT_NULL, pgv_control_thread_stack, sizeof(pgv_control_thread_stack), 4, 2); /* PGV控制线程初始化 */
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&pgv_control_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("pgv control thread error\n");
    }
}

/**
 * @description: 发送指定命令事件给PGV发线程
 * @param {type} 
 * @return: 
 */
rt_err_t PGV_Send(enum PGV_CMD_Mode _cmd)
{
    rt_err_t result;
    result = rt_mb_send(&pgv_tx_cmd_mailbox, _cmd);
    return result;
}

/**
 * @description: 返回PGV应答数据,RT_EOK 成功；-RT_ERROR 失败
 * @param {type} 
 * @return: 
 */
rt_err_t Return_PGV_Data(struct PGV_Response_Data *_response_data, rt_int32_t waittime)
{
    rt_err_t result;
    result = rt_mq_recv(&pgv_rx_data_message, _response_data, sizeof(struct PGV_Response_Data), waittime);
    return result;
}

/**
 * @description: 接收回调函数，接收到一帧数据后，释放信号量
 * @param {type} 
 * @return: 
 */
rt_err_t PGV_CallBack(rt_device_t dev, rt_size_t size)
{
    /* 保存接收数据长度 */
    pgv_rx_length = size;
    rt_sem_release(&pgv_rx_sem);
    return RT_EOK;
}

/**
 * @description: PGV控制线程，PGV为一发一收
 * @param {type} 
 * @return: 
 */
void PGV_Control_Thread(void *parameter)
{
    rt_err_t rt_result = RT_EOK;
    rt_size_t read_size = 0;

    while (1)
    {
        rt_result = rt_mb_recv(&pgv_tx_cmd_mailbox, (rt_ubase_t *)&tx_cmd, RT_WAITING_FOREVER); /* 等待发送指令 */
        if (rt_result != RT_EOK)
        {
            continue;
        }

        /* 根据指令发送数据 */
        switch (tx_cmd)
        {
        case Read_PGV_Data: /* 读取数据 */
            rt_device_write(pgv_dev, 0, "\xC8\x37", 2);
            break;
        case Set_Color_B: /* 设置色带颜色蓝色 */
            rt_device_write(pgv_dev, 0, "\xC4\x3B", 2);
            break;
        case Set_Color_G: /* 设置色带颜色绿色 */
            rt_device_write(pgv_dev, 0, "\x88\x77", 2);
            break;
        case Set_Color_R: /* 设置色带颜色红色 */
            rt_device_write(pgv_dev, 0, "\x90\x6F", 2);
            break;
        case Set_Dir_Best: /* 设置选择最优的轨道 */
            rt_device_write(pgv_dev, 0, "\xEC\x13", 2);
            break;
        case Set_Dir_L: /* 设置选择左边的轨道 */
            rt_device_write(pgv_dev, 0, "\xE8\x17", 2);
            break;
        case Set_Dir_R: /* 设置选择右边的轨道 */
            rt_device_write(pgv_dev, 0, "\xE4\x1B", 2);
            break;
        default:
            continue;
            // break;
        }

        rt_result = rt_sem_take(&pgv_rx_sem, 2000); /* 等待2s应答超时 */
        if (rt_result != RT_EOK)
        {
            /* PGV传感器应答超时，检查连线 */
            // rt_kprintf("pgv timeout\n");
            continue;
        }

        /* 接收到应答数据 */
        if (pgv_rx_length > sizeof(pgv_rx_data))
        {
            /* 长度限幅 */
            pgv_rx_length = sizeof(pgv_rx_data);
        }
        read_size = rt_device_read(pgv_dev, 0, pgv_rx_data, pgv_rx_length); /* 读取数据 */

        /* 根据指令解析数据 */
        switch (tx_cmd)
        {
        case Read_PGV_Data: /* 读取数据 */
        {
            rt_result = Analyze_Read_CMD_Data(pgv_rx_data, read_size, &measure_inpgv); /* 解析数据 */
            if (RT_EOK == rt_result)
            {
                /* 解析数据正确且读取到目标 */
                Trans_Coor(&measure_inpgv, &response_data);                                                     /* 将数据转换至AGV坐标系 */
                rt_result = rt_mq_send(&pgv_rx_data_message, &response_data, sizeof(struct PGV_Response_Data)); /* 将数据发送到消息队列，等待外部线程处理 */
            }
        }
        break;
        case Set_Color_B: /* 设置色带颜色蓝色 */
        {
            rt_result = Analyze_Set_Color(Set_Color_B, pgv_rx_data, "\x01\x01");
            if (RT_EOK == rt_result)
            {
                /* 找到应答 */
                rt_result = rt_mq_send(&pgv_rx_data_message, &response_data, sizeof(struct PGV_Response_Data)); /* 将数据发送到消息队列，等待外部线程处理 */
            }
        }
        break;
        case Set_Color_G: /* 设置色带颜色绿色 */
        {
            rt_result = Analyze_Set_Color(Set_Color_G, pgv_rx_data, "\x02\x02");
            if (RT_EOK == rt_result)
            {
                /* 找到应答 */
                rt_result = rt_mq_send(&pgv_rx_data_message, &response_data, sizeof(struct PGV_Response_Data)); /* 将数据发送到消息队列，等待外部线程处理 */
            }
        }
        break;
        case Set_Color_R: /* 设置色带颜色红色 */
        {
            rt_result = Analyze_Set_Color(Set_Color_R, pgv_rx_data, "\x04\x04");
            if (RT_EOK == rt_result)
            {
                /* 找到应答 */
                rt_result = rt_mq_send(&pgv_rx_data_message, &response_data, sizeof(struct PGV_Response_Data)); /* 将数据发送到消息队列，等待外部线程处理 */
            }
        }
        break;
        case Set_Dir_Best: /* 设置选择最优的轨道 */
        case Set_Dir_L:    /* 设置选择左边的轨道 */
        case Set_Dir_R:    /* 设置选择右边的轨道 */
        {
            rt_result = Analyze_Set_Dir(tx_cmd, pgv_rx_data, read_size);
            if (RT_EOK == rt_result)
            {
                /* 找到应答 */
                rt_result = rt_mq_send(&pgv_rx_data_message, &response_data, sizeof(struct PGV_Response_Data)); /* 将数据发送到消息队列，等待外部线程处理 */
            }
        }
        break;
        default:
            break;
        }
    }
}

/* 根据x,y计算tag号 */
rt_uint32_t PGV_Cal_TAG(int x, int y)
{
    rt_uint32_t tag_num;
    tag_num = (x + 4999) * 10000 + (y + 4999);
    return tag_num;
}

/* 根据tag号计算x,y */
void PGV_Cal_X_Y(int *x, int *y, const rt_uint32_t tag_num)
{
    *x = (tag_num / 10000) - 4999;
    *y = (tag_num % 10000) - 4999;
}
